//SERIAL API WITH I2C IMPLEMENTATION

/* INCLUSIONS */
#include "SERIALAPI.h"

//I2C Preprocessor Directives
#include "lpc17xx_i2c.h"
#include "i2c_msg_codes.h"

#define I2C_SLAVE_ADDR 0x90
#define MSG_SIZE 0x02

/* GLOBAL VARIABLES */
int8_t M1_SPEED = 0x00;
int8_t M2_SPEED = 0x00;
uint8_t TURNING = 0x00;

//I2C Globals
I2C_M_SETUP_Type master_data_cfg;
uint8_t message[MSG_SIZE];

//I2C Function Prototypes
void i2c_setup(void);
void i2c_send_msg(uint8_t, uint8_t);


/* FUNCTION BODIES */
uint8_t abs(uint8_t a, uint8_t b)
{
    return (a > b ? a - b : b - a);
}

void SERIAL_INIT(void) {
  UART_CFG_Type      UARTConfigStruct     ;
  UART_FIFO_CFG_Type UARTFIFOConfigStruct ;
  PINSEL_CFG_Type    PinCfg               ;

  UART_ConfigStructInit(&UARTConfigStruct)       ;
  UARTConfigStruct.Baud_rate = (uint32_t) 115200 ;

  UART_FIFOConfigStructInit (&UARTFIFOConfigStruct)                                ;
  UART_Init                 ((LPC_UART_TypeDef *)LPC_UART3, &UARTConfigStruct)     ;
  UART_FIFOConfig           ((LPC_UART_TypeDef *)LPC_UART3, &UARTFIFOConfigStruct) ;
  UART_TxCmd                ((LPC_UART_TypeDef *)LPC_UART3, ENABLE)                ;

  PinCfg.Funcnum   = 2      ;
  PinCfg.OpenDrain = 0      ;
  PinCfg.Pinmode   = 0      ;
  PinCfg.Portnum   = 0      ;
  PinCfg.Pinnum    = 0      ;
  PINSEL_ConfigPin(&PinCfg) ;
  PinCfg.Pinnum    = 1      ;
  PINSEL_ConfigPin(&PinCfg) ;

  UART_ConfigStructInit(&UARTConfigStruct)       ;
  UARTConfigStruct.Baud_rate = (uint32_t) 115200 ;

  UART_FIFOConfigStructInit (&UARTFIFOConfigStruct)                                ;
  UART_Init                 ((LPC_UART_TypeDef *)LPC_UART3, &UARTConfigStruct)     ;
  UART_FIFOConfig           ((LPC_UART_TypeDef *)LPC_UART3, &UARTFIFOConfigStruct) ;
  UART_TxCmd                ((LPC_UART_TypeDef *)LPC_UART3, ENABLE)                ;

  i2c_setup();
}

void i2c_setup(void){

  PINSEL_CFG_Type    PinCfg;

 //Configure second I2C device (first in use by m3pi)
  PinCfg.Funcnum     = PINSEL_FUNC_2;
  PinCfg.OpenDrain   = PINSEL_PINMODE_NORMAL;
  PinCfg.Pinmode     = PINSEL_PINMODE_PULLUP;
  PinCfg.Portnum     = 0;
  PinCfg.Pinnum      = 27;
  PINSEL_ConfigPin(&PinCfg);
  PinCfg.Pinnum      = 28;
  PINSEL_ConfigPin(&PinCfg);

  I2C_Init(LPC_I2C2, 100000);

  //Specify slave's address message buffer
  master_data_cfg.sl_addr7bit = I2C_SLAVE_ADDR;
  master_data_cfg.tx_data = message;
  master_data_cfg.tx_length = sizeof(message);
  master_data_cfg.rx_data = NULL;
  master_data_cfg.rx_length = 0;

  I2C_Cmd(LPC_I2C2, ENABLE);

}

void i2c_send_msg(uint8_t code, uint8_t param){
  //Imperative to transmit msg to slave before executing command,
  //If link fails i2c will block, thus master would become unresponsive
  message[0] = code;
  message[1] = param;
  while (!I2C_MasterTransferData(LPC_I2C2, &master_data_cfg, I2C_TRANSFER_POLLING))
    {
      //Halt & Resend until successful
      if (M1_SPEED != 0x00 || M2_SPEED != 0x00){
      uint8_t CMD_DATA[4] = {M1FBYTE, 0x00, M2FBYTE, 0x00};
      UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING) ;
      M1_SPEED = 0x00;
      M2_SPEED = 0x00;
      }
    }

}

//Motor functions are called normally by sensors and robot logic.
//I2C messages are passed to the slave on a per-function basis,
//ensuring that the slave replicates master's movements in a timely way

void MOVE_FORWARD (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_FWD, SPEED);
  uint8_t CMD_DATA[4] = {M1FBYTE, SPEED, M2FBYTE, SPEED}          ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING);
  M1_SPEED = SPEED;
  M2_SPEED = SPEED;
}

void MOVE_REVERSE (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_REV, SPEED);
  uint8_t CMD_DATA[4] = {M1RBYTE, SPEED, M2RBYTE, SPEED}           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING) ;
  M1_SPEED = (0xFF - SPEED);
  M2_SPEED = (0xFF - SPEED);
}

void STOP_MOTORS (void) {
  i2c_send_msg(I2C_MSG_STP, 0x00);
  uint8_t CMD_DATA[4] = {M1FBYTE, 0x00, M2FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 4, BLOCKING) ;
  M1_SPEED = 0x00;
  M2_SPEED = 0x00;
}

void M1_STOP(void) {
  i2c_send_msg(I2C_MSG_M1S, 0x00);
  uint8_t CMD_DATA[2] = {M1FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = 0x00;
}

void M2_STOP(void) {
  i2c_send_msg(I2C_MSG_M2S, 0x00);
  uint8_t CMD_DATA[2] = {M2FBYTE, 0x00};
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = 0x00;
}

void M1_FORWARD (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_M1F, SPEED);
  uint8_t CMD_DATA[2] = {M1FBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = SPEED;
}

void M1_REVERSE (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_M1R, SPEED);
  uint8_t CMD_DATA[2] = {M1RBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M1_SPEED = -SPEED;
}

void M2_FORWARD (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_M2F, SPEED);
  uint8_t CMD_DATA[2] = {M2FBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = SPEED;
}

void M2_REVERSE (uint8_t SPEED) {
  i2c_send_msg(I2C_MSG_M2R, SPEED);
  uint8_t CMD_DATA[2] = {M2RBYTE, SPEED}                           ;
  UART_Send((LPC_UART_TypeDef *) LPC_UART3, CMD_DATA, 2, BLOCKING) ;
  M2_SPEED = -SPEED;
}

void M1_INCREASE (uint8_t SPEED) {
  if ((M1_SPEED + SPEED) >= 0) {
    M1_FORWARD(M1_SPEED + SPEED);
  }
  else {
    M1_REVERSE(abs(M1_SPEED, SPEED));
  }
}

void M1_DECREASE (uint8_t SPEED) {
  if ((M1_SPEED - SPEED) <= 0) {
    M1_REVERSE(SPEED - M1_SPEED);
  }
  else {
    M1_FORWARD(M1_SPEED - SPEED);
  }
}

void M2_INCREASE (uint8_t SPEED) {
  if ((M2_SPEED + SPEED) >= 0) {
    M2_FORWARD(M2_SPEED + SPEED);
  }
  else {
    M2_REVERSE(abs(M2_SPEED, SPEED));
  }
}

void M2_DECREASE (uint8_t SPEED) {
  if ((M2_SPEED - SPEED) <= 0) {
    M2_REVERSE(SPEED - M2_SPEED);
  }
  else {
    M2_FORWARD(M2_SPEED - SPEED);
  }
}
